#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  Sonar,          sensorSONAR_cm)
#pragma config(Sensor, dgtl7,  LS,             sensorTouch)
#pragma config(Sensor, dgtl8,  RS,             sensorTouch)
#pragma config(Motor,  port2,           RM,            tmotorVex269, openLoop)
#pragma config(Motor,  port3,           LM,            tmotorVex269, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void forward(int revs){
 int turns=revs*360;
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
 while(SensorValue[rightEncoder]<turns){
  if(SensorValue[rightEncoder]>-SensorValue[leftEncoder]){
    motor[RM]=117;
    motor[LM]=127;
}
  else if(-SensorValue[leftEncoder]<SensorValue[rightEncoder]){
    motor[LM]=127;
    motor[RM]=117;
}
    else{
    motor[RM]=127;
    motor[LM]=127;
  }
 }
}

void backwards(int revs){
 int turns=revs*360;
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
 while(SensorValue[rightEncoder]>-turns){
  if(SensorValue[rightEncoder]>-SensorValue[leftEncoder]){
    motor[RM]=-117;
    motor[LM]=-127;
}
  else if(-SensorValue[leftEncoder]<turns){
    motor[LM]=-127;
    motor[RM]=-117;
}
    else{
    motor[RM]=-127;
    motor[LM]=-127;
  }
 }
}

void turnRight(){
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
while(SensorValue[rightEncoder]>-180){
  motor[RM]=-127;
  motor[LM]=127;
 }
}

void turnLeft(){
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
while(SensorValue[rightEncoder]<180){
  motor[RM]=127;
  motor[LM]=-127;
 }
}

void slightRight(){
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
while(SensorValue[rightEncoder]>-180){
  motor[RM]=-127;
  motor[LM]=127;
 }
}

void slightLeft(){
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
while(SensorValue[rightEncoder]<180){
  motor[RM]=127;
  motor[LM]=-127;
 }

}

void checkDirection(){
 int lCheck;
 int rCheck;
 turnLeft();
 lCheck=(SensorValue[Sonar]);
 wait1Msec(250);
 turnRight();
 wait1Msec(250);
 turnRight();
 rCheck=(SensorValue[Sonar]);
 wait1Msec(250);
 turnLeft();
 wait1Msec(250);
 if(lCheck<rCheck){
   turnRight();
 }
 else if(lCheck>rCheck){
   turnLeft();
 }
}

void pause(int wait){
  motor[RM]=0;
  motor[LM]=0;
  wait1Msec(wait);
}

task main(){
  while(1==1){
  while(SensorValue[Sonar]>75 || SensorValue[Sonar]<0){
  if(SensorValue[LS]==0 && SensorValue[RS]==0){
  forward(1);
   }
  else if(SensorValue[RS]==1){
   backwards(1);
   slightLeft();
}

 else if(SensorValue[LS]==1){
   backwards(1);
   slightRight();
   }
  }
  checkDirection();
 }
}
